#include "Sampler.h"

SingleRobotSampler::SingleRobotSampler(int robot_index,
	Polygon_2 robot_polygon,
	Room room,
	CollisionDetector *col,
	double seed) :
_robot_index(robot_index),
_col(col)
{	
	Point_2 tl = room.first;
	Point_2 br = room.second;

	double height_of_robot = CGAL::to_double(robot_polygon.top_vertex()[0].y()) - CGAL::to_double(robot_polygon.bottom_vertex()[0].y());
	double width_of_robot = CGAL::to_double(robot_polygon.right_vertex()[0].x()) - CGAL::to_double(robot_polygon.left_vertex()[0].x());

	xmin = CGAL::to_double(tl.x()) + width_of_robot;
	xmax = CGAL::to_double(br.x()) - width_of_robot;
	ymax = CGAL::to_double(br.y()) - height_of_robot;
	ymin = CGAL::to_double(tl.y()) + height_of_robot;
	lenx = xmax - xmin;
	leny = ymax - ymin;

	if (seed == -1)
		seed = time(NULL);
	srand ( seed );
}

Point_2 SingleRobotSampler::generate_sample()
{
	double randx = xmin + lenx * ((double)rand())/((double)RAND_MAX);
	double randy = ymin + leny * ((double)rand())/((double)RAND_MAX);

	Point_2 p(randx,randy);

	return p;
}

Point_2 SingleRobotSampler::generate_free_sample()
{
	while(true)
	{
		Point_2 new_config = generate_sample();

		if (_col->single_robot_valid_conf(_robot_index, new_config))
		{
			return new_config;
		}
	}
}

MultiRobotsSampler::MultiRobotsSampler(Polygon_2 robot_1_polygon,
									   Polygon_2 robot_2_polygon,
									   Room room,
									   CollisionDetector *col,
									   double seed) : 
m_col(col)
{	
	m_robot_1_sampler_ptr = new SingleRobotSampler(1, robot_1_polygon,room,col,seed);
	m_robot_2_sampler_ptr = new SingleRobotSampler(2, robot_2_polygon,room,col,seed);
}

ConfSet MultiRobotsSampler::generate_free_sample()
{
	while(true)
	{
		ConfSet new_config = generate_sample();

		if (m_col->valid_conf(new_config[0],new_config[1]))
		{
			return new_config;
		}
	}
}

ConfSet MultiRobotsSampler::generate_sample()
{
	ConfSet robots_config;

	robots_config.push_back(m_robot_1_sampler_ptr->generate_sample());
	robots_config.push_back(m_robot_2_sampler_ptr->generate_sample());

	return robots_config;
}

Point_d MultiRobotsSampler::generate_free_sample_point_d()
{
	ConfSet robots_config = generate_free_sample();

	double temp_array[] = {CGAL::to_double(robots_config[0].x()),
		CGAL::to_double(robots_config[0].y()),
		CGAL::to_double(robots_config[1].x()),
		CGAL::to_double(robots_config[1].y())};

	return Point_d(4, temp_array, temp_array + 4);
}

Point_d MultiRobotsSampler::generate_sample_point_d()
{
	ConfSet robots_config = generate_sample();

	double temp_array[] = {CGAL::to_double(robots_config[0].x()),
		CGAL::to_double(robots_config[0].y()),
		CGAL::to_double(robots_config[1].x()),
		CGAL::to_double(robots_config[1].y())};

	return Point_d(4, temp_array, temp_array + 4);
}
